#pragma ident "$Id: SolverMW.cpp 2877 2011-08-20 08:40:11Z  $"

/**
 * @file SolverMW.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/09/19
//  a general solver for MW filtering 
//
//  2019/10/05
//  use inverseDiag to replace the inverseChol
//
//  2020/02/17
//  seperate SolverMW/SolverMWAR 
//
//============================================================================


#include "SolverMW.hpp"
#include "SystemTime.hpp"
#include "SolverAmb.hpp"
#include "Equation.hpp"
#include "Variable.hpp"

#define debug 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SolverMW::classIndex = 9600001;


      // Returns an index identifying this object.
   int SolverMW::getIndex() const
   { return index; }


      // Returns a string identifying this object.
   std::string SolverMW::getClassName() const
   { return "SolverMW"; }

   void SolverMW::Init(void)
   {}

   void SolverMW::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
       // The following variables are, SourceID and SatID-indexed
       Variable BWL12GPS( TypeID::BWL12, &BWL12GPSModel, true, true, true );
       Variable BWL15GPS( TypeID::BWL15, &BWL15GPSModel, true, true, true );

       Variable BWL15GAL( TypeID::BWL15, &BWL15GALModel, true, true, true );
       Variable BWL17GAL( TypeID::BWL17, &BWL17GALModel, true, true, true );
       Variable BWL18GAL( TypeID::BWL18, &BWL18GALModel, true, true, true );
       Variable BWL16GAL( TypeID::BWL16, &BWL16GALModel, true, true, true );

       Variable BWL27BDS( TypeID::BWL27, &BWL27BDSModel, true, true, true );
       Variable BWL26BDS( TypeID::BWL26, &BWL26BDSModel, true, true, true );

       //
       // Define Equation object for code/phase observations, and add variables 
       //

       // GPS PC
       Equation equMW12GPS(TypeID::MW12);
       Equation equMW15GPS(TypeID::MW15);

       Equation equMW15GAL(TypeID::MW15, SatelliteSystem::Galileo);
       Equation equMW17GAL(TypeID::MW17, SatelliteSystem::Galileo);
       Equation equMW18GAL(TypeID::MW18, SatelliteSystem::Galileo);
       Equation equMW16GAL(TypeID::MW16, SatelliteSystem::Galileo);

       Equation equMW27BDS(TypeID::MW27, SatelliteSystem::BDS);
       Equation equMW26BDS(TypeID::MW26, SatelliteSystem::BDS);

       //////////////////////////
       // insert variables now
       //////////////////////////
       // GPS
       equMW12GPS.addVariable(BWL12GPS, WL12_WAVELENGTH_GPS);
       equMW15GPS.addVariable(BWL15GPS, WL15_WAVELENGTH_GPS);

       equMW15GAL.addVariable(BWL15GAL, WL15_WAVELENGTH_GAL);
       equMW17GAL.addVariable(BWL17GAL, WL17_WAVELENGTH_GAL);
       equMW18GAL.addVariable(BWL18GAL, WL18_WAVELENGTH_GAL);
       equMW16GAL.addVariable(BWL16GAL, WL16_WAVELENGTH_GAL);

       equMW27BDS.addVariable(BWL27BDS, WL27_WAVELENGTH_BDS);
       equMW26BDS.addVariable(BWL26BDS, WL26_WAVELENGTH_BDS);

       // define the weight for all combinations relative PC12GPS

       // Phase equations should have higher relative weight
       // var(BWL) ~= 1/2*0.3^2 
       // weight ~= 2/0.3^2 = 22
       equMW12GPS.setWeight(22.0);     // 100.0 * 100.0
       equMW15GPS.setWeight(22.0);     // 100.0 * 100.0

       equMW15GAL.setWeight(22.0);
       equMW17GAL.setWeight(22.0);
       equMW18GAL.setWeight(22.0);
       equMW16GAL.setWeight(22.0);

       equMW27BDS.setWeight(22.0);
       equMW26BDS.setWeight(22.0);

          ///////////
          // Now, the real-equation that are actived in the equationSystems.
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(satSys == SatelliteSystem::GPS)
       {
           if( mwType == TypeID::MW12 )
           {
               equSystem.addEquation(equMW12GPS);
           }
           else if ( mwType == TypeID::MW15 )
           {
               equSystem.addEquation(equMW15GPS);
           }
       }
       else if(satSys == SatelliteSystem::Galileo)
       {
           if( mwType == TypeID::MW15)
           {
               equSystem.addEquation(equMW15GAL);
           }
           else if(mwType == TypeID::MW17)
           {
               equSystem.addEquation(equMW17GAL);
           }
           else if(mwType == TypeID::MW18)
           {
               equSystem.addEquation(equMW18GAL);
           }
           else if(mwType == TypeID::MW16)
           {
               equSystem.addEquation(equMW16GAL);
           }
       }
       else if(satSys == SatelliteSystem::BDS)
       {
           if( mwType == TypeID::MW27 )
           {
               equSystem.addEquation(equMW27BDS);
           }
           else if ( mwType == TypeID::MW26 )
           {
               equSystem.addEquation(equMW26BDS);
           }
       }

   } // end of setUpEquations



      /* Returns a reference to a gnssRinex object after solving
       *  the previously defined equation system.
       *
       * @param gData    Data object holding the data.
       */
   void SolverMW::Process(commonHeader& comHeader, gnssRinex& gData)
       noexcept(false)
   {

      try
      {
         if(gData.numSats() == 0)
         {
             return;
         }
         preCompute( gData );
         Compute( gData );
         postCompute( gData );
      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );
         GPSTK_THROW(e);
      }

      return;

   }  // End of method 'SolverMW::Prepare()'



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverMW::preCompute( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {

         if(debug)
         {
             CivilTime cv(gRin.header.epoch) ;
             cout << "start of SolverMW at epoch:" << cv << endl;
         }

            // Prepare the equation system with current data
         equSystem.Prepare(gRin);

            // Get the number of unknowns being processed
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         currentUnkSet = ( equSystem.getVarUnknowns() );

         if(debug)
         {
             cout << "currentUnkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=currentUnkSet.begin();
                     vIt!=currentUnkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }


            // Get matrices and vectors out of equation system
            // Measurements vector (Prefit-residuals)
         measVector = equSystem.getPrefitsVector();
         hMatrix = equSystem.getGeometryMatrix();
         rMatrix = equSystem.getWeightsMatrix();

         if(debug)
         {
             cout << "measVector:" << endl;
             cout << measVector << endl;
             cout << hMatrix << endl;
             cout << rMatrix << endl;
         }

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

         if(debug)
         {
             YDSTime yds(gRin.header.epoch);

             cout << yds.year << " " 
                  << yds.doy  << " "
                  << yds.sod  << " " 
                  << endl;

             cout << "phiMatrix" << endl;
             cout << phiMatrix << endl;
             cout << qMatrix << endl;

             cout << "measVector" << endl;
             cout << measVector << endl;
             cout << hMatrix<< endl;
             cout << rMatrix<< endl;
         }

            // Feed the filter with the correct state and covariance matrix
         if(firstTime)
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            for( auto v: currentUnkSet )
            {
                int index = v.getNowIndex();
                initialCov(index,index) = v.getInitialVariance();
            }


               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

            // first epoch
            firstEpoch = gRin.header.epoch;

               // No longer first time
            firstTime = false;
         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            for( auto v: currentUnkSet )
            {
                int now_index = v.getNowIndex();
                int old_index = v.getPreIndex();
                if( old_index != -1)
                {
                    currentState(now_index) = solution( old_index );
                }
                else
                {
                    currentState(now_index) = 0.0;
                }
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang

               // We need a copy of 'currentUnkSet'
            VariableSet tempSet( currentUnkSet );
            for( auto v: currentUnkSet )
            {
                int nowIndex = v.getNowIndex();
                int oldIndex = v.getPreIndex();
                if( oldIndex != -1 )
                {
                    for( auto v2: tempSet )
                    {
                        int nowIndex2 = v2.getNowIndex();
                        int oldIndex2 = v2.getPreIndex();

                        if( oldIndex2 != -1 )
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = covMatrix(oldIndex, oldIndex2);
                        }
                        else
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = 0.0;
                        }
                    }
                }
                else
                {
                    currentCov(nowIndex, nowIndex) = v.getInitialVariance();
                }
                // erase var
                tempSet.erase(v);
            }  // End of for( VariableSet::const_iterator itVar1 = currentUnkSet...'


               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;

         }  // End of 'if(firstTime)'

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverMW::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   gnssRinex& SolverMW::Compute( gnssRinex& gRin )
       noexcept(false)
   {
      /////////////////////////////////////
      //
      // TimeUpdate( phiMatrix, qMatrix );
      //
      /////////////////////////////////////

      Matrix<double> phiT(transpose(phiMatrix));

         // Compute the a priori state vector
      xhatminus = phiMatrix*xhat;
         // Compute the a priori estimate error covariance matrix
      Pminus = phiMatrix*P*phiT + qMatrix;

        
      /////////////////////////////////////
      //
      // MeasUpdate( measVector, hMatrix, rMatrix );
      //
      /////////////////////////////////////
        
         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> hMatrixT( transpose(hMatrix) );

      try
      {
         // shjzhang, see MatrixOperator 
         invPMinus = inverseChol(Pminus);

         Matrix<double> invTemp( hMatrixT * rMatrix * hMatrix +
                                 invPMinus );

            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );

            // Compute the a posteriori state estimation
         xhat = P * ( (hMatrixT * rMatrix * measVector) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return gRin;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;
         // Compute the postfit residuals Vector
      postfitResiduals = measVector - (hMatrix * solution);

         // Return  
      return gRin;

   }  // End of method 'SolverMW::Compute()'


      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverMW::postCompute( gnssRinex& gRin )
       noexcept(false)
   {
      try
      {
          CommonTime epoch = gRin.header.epoch;

          // print float mw 
          if(pMWStream!=NULL)
              printObs( pMWStream, epoch, gRin);

          VariableEstDataMap ambEstData;
          ambEstData = getAmbs(solution, covMatrix);

          if(debug)
          {
             cout << "vars:" << endl; 
             for(auto v: currentUnkSet)
             {
                 cout << v << endl;
             }
          }

          if(debug)
              cout << solution << endl;

          if(debug)
              cout << covMatrix << endl;

          // insert current ambEstData into globalEstData
          for(auto v: ambEstData)
          {
              ambSizeMap[v.first]++;
              globalAmbEstData[v.first] = v.second;
          }

          if(pAmbStream!=NULL)
              printAmbs(pAmbStream, epoch, ambEstData);


      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverMW::postCompute()'

   void SolverMW::PostProcess()
       noexcept(false)
   {

     try
     {
         // loop all saved data in forward processing
         satVarEstDataMap satAmbEstData;
         for( auto amb : globalAmbEstData )
         {
             SatID sat = amb.first.getSatellite();
             satAmbEstData[sat][amb.first] = amb.second;
         }

         // now, let's find the ambiguity with the maximum arc size
         VariableEstDataMap globalAmbEstDataMinVar;
         for( auto sat : satAmbEstData )
         {
             VariableEstDataMap ambEstData = sat.second;
             
             double minVar(9999.0);
             Variable ambMinVar;
             for( auto amb: ambEstData )
             {
                 double var = amb.second.variance;
                 if( var < minVar )
                 {
                     minVar = var; 
                     ambMinVar = amb.first;
                 }
             }
             //insert max_arc ambiguity into ambEstDataMinVar
             globalAmbEstDataMinVar[ambMinVar] = ambEstData[ambMinVar] ;
         }

         YDSTime yds(firstEpoch);
         yds.sod = 0.0;

         // print ambiguities with minimum for each satellite
         if(pAmbStreamMinVar!=NULL)
            printAmbs( pAmbStreamMinVar, yds, globalAmbEstDataMinVar);

     }
     catch(Exception& u)
     {
           // Throw an exception if something unexpected happens
        ProcessingException e( getClassName() + ":"
                               + StringUtils::asString( getIndex() ) + ":"
                               + u.what() );

        GPSTK_THROW(e);

     }

       return;
   }

   VariableEstDataMap SolverMW::getAmbs(Vector<double>& solution,
                                        Matrix<double>& covMatrix)
   {
       VariableEstDataMap ambEstData;
       for(auto v: currentUnkSet)
       {
           if(v.getArcIndexed())
           {
               ambEstData[v].value = solution(v.getNowIndex());
               ambEstData[v].variance = covMatrix(v.getNowIndex(), v.getNowIndex());
           }
       }
       return ambEstData;
   };


   // print mw values
   void SolverMW::printObs( std::ofstream* outStr, 
                            const CommonTime& epoch, 
                            gnssRinex& gData)
   {
       SourceID source = gData.header.source;
       satTypeValueMap stvData = gData.body;
       for(auto stv: stvData)
       {
           double value;
           if(stv.first.system==SatelliteSystem::GPS)
           {
               if(mwType==TypeID::MW12)
               {
                  value = stv.second[mwType]/WL12_WAVELENGTH_GPS ;
               }
           }
           double satArc = stv.second[TypeID::satArc];
           double frac  = value - std::floor(value+0.5);
              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
           *outStr << source << " "
                   << stv.first  << " "
                   << satArc << " "
                   << mwType << " "
                   << rightJustify(asString(value,6),10) << " "
                   << rightJustify(asString(frac,6),10) << " "
                   << endl;
       }
   }

   // print mw values
   void SolverMW::printAmbs( std::ofstream* outStr, 
                             const CommonTime& epoch, 
                             VariableEstDataMap& ambEstData)
   {
       for(auto amb: ambEstData )
       {
           SourceID source = amb.first.getSource();
           SatID sat       = amb.first.getSatellite();
           TypeID type     = amb.first.getType();
           double satArc   = amb.first.getArc(); 
           double value    = amb.second.value;
           double var      = amb.second.variance;
           double frac     = value - std::floor(value+0.5);

           int size = ambSizeMap[amb.first];

              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
           *outStr << source << " "
                   << sat << " "
                   << rightJustify(asString<int>(int(satArc)),6) << " "
                   << type << " "
                   << rightJustify(asString(value,6),13) << " "
                   << rightJustify(asString(var,6),13) << " "
                   << rightJustify(asString(frac,6),13) << " "
                   << rightJustify(asString<int>(int(size)),6) << " "
                   << endl;
       }
   }

}  // End of namespace gpstk
